#include "rads_drive.hpp"

/*
   以0.5 m/s的速度向前移动，且以0.5 rad/s的速度绕z轴旋转（向右转）
   rostopic pub /sradsdrivetwist geometry_msgs/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}"
 */

// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;
void printCurrentTime() {
    // 获取当前时间点
    auto now = std::chrono::system_clock::now();
    // 转换为time_t类型，这是C标准库日期和时间函数使用的类型
    std::time_t now_time = std::chrono::system_clock::to_time_t(now);
    // 使用tm结构体来表示本地时间
    std::tm* local_time = std::localtime(&now_time);
    // 打印日期和时间
    std::cout << std::put_time(local_time, "%Y-%m-%d %H:%M:%S") << std::endl;
}
void printData(const uint8_t* data, size_t size) {
    printCurrentTime();
    for (size_t i = 0; i < size; ++i) {
        // 以十六进制格式打印每个字节，并设置字段宽度为2
        std::cout << std::setw(2) << std::setfill('0') << std::hex << static_cast<int>(data[i]) << " ";
        // 如果需要换行，可以在每打印16个字节后添加一个换行符
        if ((i + 1) % 25 == 0) {
            std::cout << std::endl;
        }
    }
    // 如果最后一行不是16的倍数，添加一个换行符
   if (size % 16 != 0) {
        std::cout << std::endl;
    }
}
/* 线程开启读取串口信息 */
void threaddrive_read(SerialPortDrive& seri,FdriveData& fdd,rads::rads_scmd& fdd_msg, PlanningControl& planctr,SerialHalfDuplex& halfDuplex)
{
    long i = 1;
	uint8_t data[DRIVE_BUFFER_SIZE];
    int serial_status = -1;
	while(!should_stop.load())
	{
        ROS_WARN("before read %ld",i);
        /*
        if(halfDuplex.isSending.load()){
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            continue;
        }
         */
        serial_status = seri.readData(data,DRIVE_BUFFER_SIZE);
        if(0 == serial_status)
        {
            fdd = seri.toDriveData(data,DRIVE_BUFFER_SIZE);
            DriveFeed2MSG(fdd,fdd_msg);
            serial_status = -1;
        }else if(-2 == serial_status)
        {
             ROS_ERROR("[drive] The serial port data XOR verification fails.");
        }
        ROS_ERROR("after read %ld",i++);
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        /*
	    std::unique_lock<std::mutex> lck(mtx);
	    cv.wait_for(lck, std::chrono::milliseconds(10), [] { return should_stop.load(); });
         */
	}
    if(seri.isOpen()) seri.close();
}

/* 线程开启串口写入信息 20ms/50hz*/
void threaddrive_write(SerialPortDrive& seri, PlanningControl& planctr,SerialHalfDuplex& halfDuplex)
{
    long i = 1;
    bool sendResult = false;
	while(!should_stop.load())
	{
        try{
	        if(seri.isOpen()){
                ROS_WARN(">>> before write %ld",i);
                /*
                std::lock_guard<std::mutex> lock(halfDuplex.sendMutex);
                halfDuplex.isSending.store(true);
                 */
                seri.writeData(reinterpret_cast<const char*>(planctr.data),DRIVE_FULL_SIZE);
        //      halfDuplex.isSending.store(false);
                ROS_ERROR(">>> after write %ld",i++);
            }
        }catch (const std::exception& e) {
            ROS_ERROR("Exception caught in threaddrive_write: %s", e.what());
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        /*
	    std::unique_lock<std::mutex> lck(mtx);
	    cv.wait_for(lck, std::chrono::milliseconds(20), [] { return should_stop.load(); });
         */
	}
    if(seri.isOpen()) seri.close();
}

// 阻塞式读写一体
void threaddrive_rw(SerialPortDrive& seri,FdriveData& fdd,rads::rads_scmd& fdd_msg, PlanningControl& planctr)
{
   
    uint8_t data[DRIVE_BUFFER_SIZE];
    int serial_status = -1;
    serial::Serial ROS_MCU(DRIVE_DEVICE, B115200, serial::Timeout::simpleTimeout(1000));//
    while(ros::ok())
    {
        // read
       ROS_MCU.read(data, 1); // 返回值是多少个u8
        if (data[0] == 255)
        {
            ROS_MCU.read(data, 1);
            if(data[0] == 254) {
                ROS_MCU.read(data,DRIVE_BUFFER_SIZE);
                //std::cout <<"data"<< std::endl;
                //printData(data,DRIVE_BUFFER_SIZE);
                // convert to fdd
                fdd = seri.toDriveData(data,DRIVE_BUFFER_SIZE);
                 DriveFeed2MSG(fdd,fdd_msg);
            }
        }
       
         /**serial_status = seri.readData(data,DRIVE_BUFFER_SIZE);
        std::cout <<"read"<< std::endl;
        printData(planctr.data,DRIVE_BUFFER_SIZE);
        if(0 == serial_status)
        {
            fdd = seri.toDriveData(data,DRIVE_BUFFER_SIZE);
            DriveFeed2MSG(fdd,fdd_msg);
            serial_status = -1;
        } **/
        // write
        seri.writeData(reinterpret_cast<const char*>(planctr.data),DRIVE_FULL_SIZE);
        //std::cout <<"write"<< std::endl;
        //printData(planctr.data,DRIVE_FULL_SIZE);
        //usleep(50000); //20hz
    }
}

// 信号处理函数
void signalHandler(int signum) {
    if(SIGINT == signum)
    {
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        ROS_INFO("[Drive] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Drive] Starting...");
    std::atomic<bool> drive_running(false);
    FdriveData fdd;
    FcontrolData fcd;
    rads::rads_scmd fdd_msg;
    std::thread t_driver,t_drivew,t_driverw;
    SerialHalfDuplex halfDuplex;
    halfDuplex.isSending.store(false);
    ////////////////////////////////////////////////////////////////////////////
	ros::init(argc, argv, NODE_DRIVE);
	ros::NodeHandle nh;
    ros::param::get("DRIVE_DEVICE",DRIVE_DEVICE);
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
    PlanningControl planctr;
    ros::Subscriber sub_ackermann = nh.subscribe("/sradsdriveackermann", QUEUE_SIZE, &PlanningControl::ackermannCallback,&planctr);
    ros::Subscriber sub_twist = nh.subscribe("/sradsdrivetwist", QUEUE_SIZE,&PlanningControl::twistCallback,&planctr);
	SerialPortDrive seri(DRIVE_DEVICE, B115200);
    while(!drive_running && !should_stop.load())
    {
        if (seri.open())
        {
            drive_running.store(true);
        }else{
            ROS_WARN("[drive] is running,but serial open [%s] fail.",std::string(DRIVE_DEVICE).c_str());
        }
        std::this_thread::sleep_for(std::chrono::seconds(DRIVE_CHECK_SEC));
    }
	if(drive_running)
	{
        #ifdef DEV_LOCAL
	    t_driver = std::thread(threaddrive_read,std::ref(seri),std::ref(fdd),std::ref(fdd_msg),std::ref(planctr),std::ref(halfDuplex));
	    t_drivew = std::thread(threaddrive_write,std::ref(seri),std::ref(planctr),std::ref(halfDuplex));
        #endif
        t_driverw = std::thread(threaddrive_rw,std::ref(seri),std::ref(fdd),std::ref(fdd_msg),std::ref(planctr));
    }
    ////////////////////////////////////////////////////////////////////////////
    ros::Publisher p = nh.advertise<rads::rads_scmd>("/pradsscmd", QUEUE_SIZE);
    ros::Rate loop_rate(50);
    while (ros::ok() && !should_stop.load())
    {
        p.publish(fdd_msg);
    	ros::spinOnce();
    	loop_rate.sleep();
    }
    if(t_driver.joinable())t_driver.join();
    if(t_drivew.joinable())t_drivew.join();
    if(t_driverw.joinable())t_driverw.join();
	return 0;
}
